#include "frame_processor.h"
#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <chrono>
#include <iostream>

FrameProcessor::FrameProcessor() 
    : use_gpu_(false), target_resolution_(1920, 1080) {
    
    // 初始化特征检测器
    feature_detector_ = cv::ORB::create(1000);
    descriptor_extractor_ = cv::ORB::create(1000);
    
    // 初始化立体匹配器
    stereo_matcher_ = cv::StereoSGBM::create(
        0, 16, 3, 8*3*3*3, 32*3*3*3, 1, 0, 10, 100, 32
    );
}

ProcessingResult FrameProcessor::process_frame(const cv::Mat& input_frame) {
    auto start_time = std::chrono::high_resolution_clock::now();
    
    ProcessingResult result;
    
    // 预处理
    cv::Mat preprocessed = preprocess_frame(input_frame);
    
    // 特征检测
    cv::Mat features = detect_features(preprocessed);
    
    // 边缘检测和轮廓
    cv::Mat gray;
    cv::cvtColor(preprocessed, gray, cv::COLOR_BGR2GRAY);
    cv::Mat edges;
    cv::Canny(gray, edges, 50, 150);
    
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    
    // 绘制结果
    result.processed_frame = preprocessed.clone();
    cv::drawContours(result.processed_frame, contours, -1, cv::Scalar(0, 255, 0), 2);
    
    // 添加信息文本
    std::string info_text = "Contours: " + std::to_string(contours.size());
    cv::putText(result.processed_frame, info_text, cv::Point(10, 30), 
                cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2);
    
    // 三维重建
    result.point_cloud = extract_3d_points(preprocessed);
    result.depth_map = estimate_depth(preprocessed);
    
    result.contours_count = static_cast<int>(contours.size());
    
    // 计算处理时间
    auto end_time = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
    result.processing_time_ms = duration.count() / 1000.0;
    
    return result;
}

cv::Mat FrameProcessor::preprocess_frame(const cv::Mat& frame) {
    cv::Mat processed = frame.clone();
    
    // 调整分辨率
    if (frame.size() != target_resolution_) {
        cv::resize(processed, processed, target_resolution_);
    }
    
    // 增强对比度
    processed = enhance_contrast(processed);
    
    // 降噪
    cv::GaussianBlur(processed, processed, cv::Size(3, 3), 0);
    
    return processed;
}

cv::Mat FrameProcessor::enhance_contrast(const cv::Mat& frame) {
    cv::Mat enhanced;
    cv::convertScaleAbs(frame, enhanced, 1.2, 10);
    return enhanced;
}

cv::Mat FrameProcessor::detect_features(const cv::Mat& frame) {
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
    
    std::vector<cv::KeyPoint> keypoints;
    cv::Mat descriptors;
    
    feature_detector_->detect(gray, keypoints);
    descriptor_extractor_->compute(gray, keypoints, descriptors);
    
    // 绘制特征点
    cv::Mat result = frame.clone();
    cv::drawKeypoints(result, keypoints, result, cv::Scalar(0, 0, 255));
    
    return result;
}

std::vector<cv::Point3f> FrameProcessor::extract_3d_points(const cv::Mat& frame) {
    std::vector<cv::Point3f> points_3d;
    
    // 这里实现你的三维重建算法
    // 示例：基于特征点的简单三维重建
    
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
    
    std::vector<cv::KeyPoint> keypoints;
    feature_detector_->detect(gray, keypoints);
    
    // 假设相机内参（实际应用中需要标定）
    double fx = 1000.0, fy = 1000.0;
    double cx = frame.cols / 2.0, cy = frame.rows / 2.0;
    
    for (const auto& kp : keypoints) {
        // 简单的深度估计（实际应用中需要更复杂的算法）
        double depth = 1.0 + (kp.response / 1000.0);
        
        // 反投影到3D空间
        double x = (kp.pt.x - cx) * depth / fx;
        double y = (kp.pt.y - cy) * depth / fy;
        double z = depth;
        
        points_3d.emplace_back(x, y, z);
    }
    
    return points_3d;
}

cv::Mat FrameProcessor::estimate_depth(const cv::Mat& frame) {
    // 这里实现深度估计算法
    // 示例：基于边缘的简单深度估计
    
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
    
    cv::Mat depth_map = cv::Mat::zeros(gray.size(), CV_32F);
    
    // 基于图像梯度的简单深度估计
    cv::Mat grad_x, grad_y;
    cv::Sobel(gray, grad_x, CV_32F, 1, 0);
    cv::Sobel(gray, grad_y, CV_32F, 0, 1);
    
    cv::Mat gradient_magnitude;
    cv::magnitude(grad_x, grad_y, gradient_magnitude);
    
    // 归一化深度图
    cv::normalize(gradient_magnitude, depth_map, 0, 1, cv::NORM_MINMAX);
    
    return depth_map;
}

std::vector<cv::KeyPoint> FrameProcessor::detect_keypoints(const cv::Mat& frame) {
    std::vector<cv::KeyPoint> keypoints;
    feature_detector_->detect(frame, keypoints);
    return keypoints;
}

cv::Mat FrameProcessor::compute_descriptors(const cv::Mat& frame, const std::vector<cv::KeyPoint>& keypoints) {
    cv::Mat descriptors;
    descriptor_extractor_->compute(frame, keypoints, descriptors);
    return descriptors;
}

cv::Mat FrameProcessor::stereo_reconstruction(const cv::Mat& left_frame, const cv::Mat& right_frame) {
    cv::Mat gray_left, gray_right;
    cv::cvtColor(left_frame, gray_left, cv::COLOR_BGR2GRAY);
    cv::cvtColor(right_frame, gray_right, cv::COLOR_BGR2GRAY);
    
    cv::Mat disparity;
    stereo_matcher_->compute(gray_left, gray_right, disparity);
    
    return disparity;
}

cv::Mat FrameProcessor::structure_from_motion(const std::vector<cv::Mat>& frames) {
    // 这里实现 Structure from Motion 算法
    // 需要多帧图像进行三维重建
    
    if (frames.size() < 2) {
        return cv::Mat();
    }
    
    // 简化的 SfM 实现
    cv::Mat result = frames[0].clone();
    
    // 在实际应用中，这里会包含：
    // 1. 特征点检测和匹配
    // 2. 本质矩阵计算
    // 3. 相机姿态估计
    // 4. 三角化
    // 5. Bundle Adjustment
    
    return result;
}

void FrameProcessor::set_processing_mode(bool enable_gpu) {
    use_gpu_ = enable_gpu;
    // 根据 GPU 模式调整算法参数
}

void FrameProcessor::set_resolution(int width, int height) {
    target_resolution_ = cv::Size(width, height);
} 